#include "vector_angles.hpp"

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <math.h>
#include <stdio.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// 向量结构体定义已在头文件中定义

// 角度转换函数已在头文件中定义
// 计算最大绝对值
double max_absolute_value(double a, double b, double c) {
    double abs_a = fabs(a);
    double abs_b = fabs(b);
    double abs_c = fabs(c);

    double max_abs = abs_a;
    if (abs_b > max_abs) max_abs = abs_b;
    if (abs_c > max_abs) max_abs = abs_c;

    return max_abs;
}
// 向量计算函数 - 简单优化版本
VectorAngles calculate_vector_angles_simple(double x, double y, double z) {
    VectorAngles result;

    // 计算向量长度
    result.length = sqrt(x*x + y*y + z*z);

    // 计算XY平面角度 (0-360度)
    if (fabs(x) < 1e-6 && fabs(y) < 1e-6) {
        result.a = 0.0;
    } else {
        double a_rad = atan2(y, x);
        result.a = degrees(a_rad);
        if (result.a < 0.0) result.a += 360.0;
    }

    // 计算与XY平面夹角
    if (fabs(result.length) < 1e-6) {
        result.b = 0.0;
    } else {
        result.b = degrees(asin(z / result.length));
    }

    return result;
}

// 打印角度信息
// void print_angles_simple(const char* label, double x, double y, double z, VectorAngles angles) {
//     char buffer[256];

//     snprintf(buffer, sizeof(buffer), "%s:", label);
//     PX4_INFO("%s", buffer);

//     snprintf(buffer, sizeof(buffer), "  向量: (%.2f, %.2f, %.2f)", x, y, z);
//     PX4_INFO("%s", buffer);

//     snprintf(buffer, sizeof(buffer), "  XY平面角度: %.2f度", angles.a);
//     PX4_INFO("%s", buffer);

//     snprintf(buffer, sizeof(buffer), "  垂直角度: %.2f度", angles.b);
//     PX4_INFO("%s", buffer);

//     snprintf(buffer, sizeof(buffer), "  向量长度: %.2f", angles.length);
//     PX4_INFO("%s", buffer);
// }


VectorAngles VectorControlWithResult(int x, int y, int z) {
    // 直接计算并显示角度
    if (z > 0) {
        z = 0;
    } else if (z < 0) {
        z = -z;
    }

    VectorAngles original = calculate_vector_angles_simple(x, -y, z);
    // print_angles_simple("原始角度", x, y, z, original); // 暂时注释掉，避免PX4_INFO问题

    if (original.a < 180) {
        if (original.b > 90) {
            original.b = 180 - original.b;
        }
    }

    if (original.a > 180) {
        original.a = original.a - 180;
        if (original.b < 90) {
            original.b = 180 - original.b;
        }
    }

    original.output = max_absolute_value(x, y, z);
    //------------------------
    //此处可添加函数用于驱动运动，original.a为大环的目标角度，original.b为小环的目标，original.output为运动速度百分比
    //------------------------
    return original;
}
